#include "buaa/formation.h"

double Formation_Agent::getHeu(GridMapNode *node1, GridMapNode *node2) {
    return tie_breaker_ * getDiagHeu(node1, node2);
}

double Formation_Agent::getDiagHeu(GridMapNode *node1, GridMapNode *node2) {
    double dx = abs(node1->index(0) - node2->index(0));
    double dy = abs(node1->index(1) - node2->index(1));
    double dz = abs(node1->index(2) - node2->index(2));

    double h = 0.0;
    int diag = std::min(std::min(dx, dy), dz);
    dx -= diag;
    dy -= diag;
    dz -= diag;

    if (dx == 0) {
        h = 1.0 * sqrt(3.0) * diag + sqrt(2.0) * std::min(dy, dz) + 1.0 * abs(dy - dz);
    }
    if (dy == 0) {
        h = 1.0 * sqrt(3.0) * diag + sqrt(2.0) * std::min(dx, dz) + 1.0 * abs(dx - dz);
    }
    if (dz == 0) {
        h = 1.0 * sqrt(3.0) * diag + sqrt(2.0) * std::min(dx, dy) + 1.0 * abs(dx - dy);
    }
    return h;
}

void Formation_Agent::visual_a_star_path() {
    nav_msgs::Path a_star_path;
    a_star_path.poses.clear();

    a_star_path.header.stamp = ros::Time::now();
    a_star_path.header.frame_id = "map";

    if (a_star_gridpath.size() == 0) return;
    for (int i = 0; i < a_star_gridpath.size(); i++) {
        geometry_msgs::PoseStamped point;

        Eigen::Vector3i path_idx = a_star_gridpath[i];
        // std::cout<<"path_idx: "<<path_idx<<std::endl;
        Eigen::Vector3d path_point = gvmcb.getMapGlobalPosfromLocalIdx(path_idx);

        point.pose.position.x = path_point(0);
        point.pose.position.y = path_point(1);
        point.pose.position.z = path_point(2);

        point.header.stamp = ros::Time::now();
        point.header.frame_id = "map";
        a_star_path.poses.push_back(point);
    }
    for (int i = 0; i < 3; i++) {
        a_star_pub.publish(a_star_path);
    }


}


void Formation_Agent::a_star_retrievePath(GridMapNode *final_node) {
    a_star_gridpath.clear();
    a_star_gridpath.push_back(final_node->index);

    while (final_node->parent_node != NULL) {
        final_node = final_node->parent_node;
        a_star_gridpath.push_back(final_node->index);
    }
}

double Formation_Agent::getpointdistance(Eigen::Vector3i idx1, Eigen::Vector3i idx2) {
    double tmp = (idx1 - idx2).cast<double>().norm();
    return tmp;
}

void Formation_Agent::a_star_RemoveCollinearPath() {
    std::vector<Eigen::Vector3i> tmp_path = a_star_gridpath;
    a_star_gridpath.clear();

    a_star_gridpath.push_back(*tmp_path.begin());

    for (std::vector<Eigen::Vector3i>::const_iterator iter = tmp_path.begin(); iter != tmp_path.end() - 2; iter++) {

        double distance13 = getpointdistance(*(iter + 2), *iter);
        double distance12 = getpointdistance(*(iter + 1), *iter);
        double distance23 = getpointdistance(*(iter + 2), *(iter + 1));
        if (abs(distance13 - distance12 - distance23) == 0) {
            // ROS_INFO("distance %f %f %f",distance13,distance12,distance23);
            continue;
        }
        a_star_gridpath.push_back(*(iter + 1));
    }
    a_star_gridpath.push_back(*(tmp_path.end() - 1));

    reverse(a_star_gridpath.begin(), a_star_gridpath.end());
    // ROS_INFO("a_star_grid_path size after remove collinear point: %ld",a_star_gridpath.size());
}


void Formation_Agent::a_star_bresenham3D() {
    std::vector<Eigen::Vector3i> tmp_path = a_star_gridpath;
    a_star_gridpath.clear();
    a_star_gridpath.push_back(*tmp_path.begin());

    Eigen::Vector3i bresenham3Dstart_idx = *tmp_path.begin();

    int j = 0;
    for (j = 1; j < (int) tmp_path.size(); j++) {
        std::vector<Eigen::Vector3i> bresenham3Dpath = getbresenham3D(bresenham3Dstart_idx, tmp_path[j]);
        for (int i = 0; i < (int) bresenham3Dpath.size(); i++) {
            int tmp = i * 1;
            if (tmp >= (int) bresenham3Dpath.size()) break;
            if (gvmcb.getMapLocalIdxEdt(bresenham3Dpath[i]) < SAFETY_DISTANCE) {
                a_star_gridpath.push_back(tmp_path[j - 1]);
                bresenham3Dstart_idx = tmp_path[j - 1];
                break;
            }
        }
    }
    a_star_gridpath.push_back(*(tmp_path.end() - 1));
    // ROS_INFO("a_star_grid_path size after bresenham3D: %ld",a_star_gridpath.size());
}


void Formation_Agent::initGridMap() {
    Eigen::Vector3i POOL_SIZE_(pool_size, pool_size, pool_size_z);

    GridMap_ = new GridMapNodePtr **[POOL_SIZE_(0)];
    for (int i = 0; i < POOL_SIZE_(0); i++) {
        GridMap_[i] = new GridMapNodePtr *[POOL_SIZE_(1)];
        for (int j = 0; j < POOL_SIZE_(1); j++) {
            GridMap_[i][j] = new GridMapNodePtr[POOL_SIZE_(2)];
            for (int k = 0; k < POOL_SIZE_(2); k++) {
                GridMap_[i][j][k] = new GridMapNode;
            }
        }
    }
}

bool Formation_Agent::a_star_search(Eigen::Vector3d start_p, Eigen::Vector3d end_p) {
    a_star_gridpath.clear();

    ros::Time time_1 = ros::Time::now();

    rounds_++;
    std::priority_queue<GridMapNode *, std::vector<GridMapNode *>, NodeComparator> empty;
    std::priority_queue<GridMapNode *, std::vector<GridMapNode *>, NodeComparator> openSet_;

    openSet_.swap(empty);

    Eigen::Vector3i start_index;
    Eigen::Vector3i end_index;

    gvmcb.getmapIdxLocal(start_p, start_index);
    gvmcb.getmapIdxLocal(end_p, end_index);

    // ROS_INFO("start_index->index: %d %d %d start_p: %f %f %f",start_index(0),start_index(1),start_index(2),start_p(0),start_p(1),start_p(2));
    // ROS_INFO("end_index->index: %d %d %d end_p: %f %f %f",end_index(0),end_index(1),end_index(2),end_p(0),end_p(1),end_p(2));

    Eigen::Vector3i centre(pool_size / 2, pool_size / 2, pool_size_z / 2);
    Eigen::Vector3i offset = start_index - centre;

    GridMapNode *start_node = GridMap_[pool_size / 2][pool_size / 2][pool_size_z / 2];
    GridMapNode *end_node = new GridMapNode;
    start_node->index = start_index;
    end_node->index = end_index;

    // if(gvmcb.getMapLocalIdxEdt(start_index)<SAFETY_DISTANCE)
    // {
    //     a_star_flag=false;
    //     ROS_WARN("start point is unsafe");
    //     return false;
    // }

    // if(gvmcb.getMapLocalIdxEdt(end_index)<SAFETY_DISTANCE)
    // {
    //     ROS_WARN("end point is unsafe");
    //     return false;
    // }

    start_node->rounds = rounds_;
    start_node->g_score = 0;

    start_node->f_score = getHeu(start_node, end_node);
    start_node->state = GridMapNode::OPENSET; //put start node in open set
    start_node->parent_node = NULL;

    openSet_.push(start_node); //put start in open set
    int num_iter = 0;

    GridMapNode *current_node;
    GridMapNode *neighborPtr;

    // ROS_INFO("GO INTO A STAR LOOP");

    if (start_index == end_index) {
        a_star_gridpath.clear();
        a_star_gridpath.push_back(start_index);
        a_star_gridpath.push_back(end_index);

        ROS_ERROR("IN A STAR FUNCTIOIN AT GOAL!!!!");
        a_star_flag = true;
        return true;

    }

    while (!openSet_.empty()) {
        num_iter++;
        current_node = openSet_.top();
        // ROS_INFO("CURRENT_NODE: %d %d %d",current_node->index(0),current_node->index(1),current_node->index(2));

        openSet_.pop();

        Eigen::Vector3d current_node_position = gvmcb.getMapGlobalPosfromLocalIdx(current_node->index);

        if ((current_node->index == end_node->index) || (start_p - current_node_position).norm() >= local_length) {
            ROS_INFO_THROTTLE(4, " [A_star]: FIND A STAR PATH");
            a_star_retrievePath(current_node);
            a_star_RemoveCollinearPath();
            a_star_bresenham3D();
            // corridor_generation();
            local_goal = current_node_position;
            a_star_flag = true;
            break;
        }

        current_node->state = GridMapNode::CLOSEDSET; //move current node from open set to closed set.

        for (int dx = -1; dx <= 1; dx++) {
            for (int dy = -1; dy <= 1; dy++) {
                for (int dz = -1; dz <= 1; dz++) {
                    if (dx == 0 && dy == 0 && dz == 0) continue;
                    Eigen::Vector3i neighborIdx;
                    neighborIdx(0) = (current_node->index)(0) + dx;
                    neighborIdx(1) = (current_node->index)(1) + dy;
                    neighborIdx(2) = (current_node->index)(2) + dz;

                    Eigen::Vector3i ConvertNeighborIdx = neighborIdx - offset;
                    if (ConvertNeighborIdx(0) >= pool_size || ConvertNeighborIdx(1) >= pool_size ||
                        ConvertNeighborIdx(2) >= pool_size_z
                        || ConvertNeighborIdx(0) <= 0 || ConvertNeighborIdx(1) <= 0 || ConvertNeighborIdx(2) <= 0)
                        continue;


                    neighborPtr = GridMap_[neighborIdx(0) - offset(0)][neighborIdx(1) - offset(1)][neighborIdx(2) -
                                                                                                   offset(2)];
                    neighborPtr->index = neighborIdx;

                    bool flag_explored = neighborPtr->rounds == rounds_;
                    // ROS_INFO("flag_explored: %d",flag_explored);

                    if (flag_explored && neighborPtr->state == GridMapNode::CLOSEDSET) {
                        continue; //in closed set.
                    }
                    neighborPtr->rounds = rounds_;

                    if (gvmcb.getMapLocalIdxEdt(neighborIdx) < SAFETY_DISTANCE) {
                        continue;
                    }

                    // Eigen::Vector3d path_point=gvmcb.getMapGlobalPosfromLocalIdx(current_node->index);
                    // if(path_point(2)>1.0+global_goal(2))
                    //     continue;

                    double tentative_gScore = current_node->g_score + dx * dx + dy * dy + dz * dz;


                    if (!flag_explored) {
                        //discover a new nodeAsyncSpinner
                        neighborPtr->state = GridMapNode::OPENSET;
                        neighborPtr->parent_node = current_node;
                        neighborPtr->g_score = tentative_gScore;
                        neighborPtr->f_score = tentative_gScore + getHeu(neighborPtr, end_node);
                        openSet_.push(neighborPtr); //put neighbor in open set and record it.
                        // ROS_INFO("neighborPtr->index: %d %d %d",neighborPtr->index(0),neighborPtr->index(1),neighborPtr->index(2));
                        if (neighborPtr->index == current_node->index)return true;

                    } else {
                        if (tentative_gScore < neighborPtr->g_score) {
                            neighborPtr->parent_node = current_node;
                            neighborPtr->g_score = tentative_gScore;
                            neighborPtr->f_score = tentative_gScore + getHeu(neighborPtr, end_node);
                        }
                    }
                }
            }
        }
        ros::Time time_2 = ros::Time::now();
        if ((time_2 - time_1).toSec() > 0.2) {
            a_star_flag = false;
            ROS_WARN("Failed in A star path searching !!! 0.2 seconds time limit exceeded.");
            break;
        }
    }

    delete end_node;

    if (a_star_gridpath.size() == 0) {
        a_star_flag = false;
        ROS_WARN("grid size ==0 astar failed");
    }
    ros::Time time_3 = ros::Time::now();
    // ROS_INFO("\033[1;%dm  a_star_flag: %d Time consume in A star path finding is %fs, iter=%d  size: %ld \033[0m",COLOR_ID, a_star_flag,(time_3 - time_1).toSec(), num_iter,a_star_gridpath.size());
    visual_a_star_path();
    return a_star_flag;

}

void Formation_Agent::MapVisualCallback(const ros::TimerEvent &e) {
    if (!gvmInitial) return;

    pcl::PointCloud<pcl::PointXYZ>::Ptr gvmcbMap_pc(new pcl::PointCloud<pcl::PointXYZ>());
    pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_edf_field(new pcl::PointCloud<pcl::PointXYZRGB>());

    int visual_map_range_idx = visual_map_range / gvmcb.resolution_;
    for (int i = 0; i < gvmcb.indexRange(2); i++) {
        for (int j = 0; j < gvmcb.indexRange(1); j++) {
            for (int k = 0; k < gvmcb.indexRange(0); k++) {
                Eigen::Vector3i idx(k, j, i);
                if (gvmcb.getMapLocalIdxOccupied(idx)) {
                    Eigen::Vector3d tmp = gvmcb.getMapGlobalPosfromLocalIdx(idx);
                    // ROS_INFO("idx: %d %d %d",idx(0),idx(1),idx(2));
                    // ROS_INFO("tmp %f %f %f",tmp(0),tmp(1),tmp(2));
                    pcl::PointXYZ vis_point;
                    vis_point.x = tmp(0);
                    vis_point.y = tmp(1);
                    vis_point.z = tmp(2);
                    gvmcbMap_pc->points.push_back(vis_point);
                }
                if (i == gvmcb.indexRange(2) / 2) {
                    Eigen::Vector3d tmp = gvmcb.getMapGlobalPosfromLocalIdx(idx);
                    pcl::PointXYZRGB vis_point;
                    vis_point.x = tmp(0);
                    vis_point.y = tmp(1);
                    vis_point.z = tmp(2);
                    Eigen::Vector3i point_RGB = gvmcb.getLocalIdxMapEdtColor(idx);
                    vis_point.r = point_RGB(0);
                    vis_point.g = point_RGB(1);
                    vis_point.b = point_RGB(2);
                    cloud_edf_field->points.push_back(vis_point);
                }
            }
        }
    }
    sensor_msgs::PointCloud2 gvmcbMap_ros;
    pcl::toROSMsg(*gvmcbMap_pc, gvmcbMap_ros);
    gvmcbMap_ros.header.stamp = ros::Time::now();
    gvmcbMap_ros.header.frame_id = "map";
    gvmcb_map_pub.publish(gvmcbMap_ros);

    sensor_msgs::PointCloud2 edf_ros_cloud;
    pcl::toROSMsg(*cloud_edf_field, edf_ros_cloud);
    edf_ros_cloud.header.stamp = ros::Time::now();
    edf_ros_cloud.header.frame_id = "map";
    cloud_edf_pub.publish(edf_ros_cloud);

}

void Formation_Agent::init(ros::NodeHandle &nh) {
    initGridMap();

    state_sub = nh.subscribe("/mavros/state", 1, &Formation_Agent::uavmodecallback, this);
    pose_sub = nh.subscribe("/mavros/local_position/pose", 1, &Formation_Agent::poseCallback, this);
    cloud_sub = nh.subscribe("/iris/camera/depth/points", 1, &Formation_Agent::cloudCallback, this);
    // 指定目标点
    global_goal_sub = nh.subscribe("/plan/end_point", 1, &Formation_Agent::GlobalGoalCallback, this);
    // TODO： 相机转角
    gimbal_angle_sub = nh.subscribe("/iris/gimbal_yaw_angle", 1, &Formation_Agent::gimbal_yaw_angleCallback, this);

    setpoint_pub = nh.advertise<geometry_msgs::PoseStamped>("/mavros/setpoint_position/local", 1);
    gvmcb_map_pub = nh.advertise<sensor_msgs::PointCloud2>("/grid_map", 1, true);     //visualization grid map
    cloud_edf_pub = nh.advertise<sensor_msgs::PointCloud2>("/esdf_map", 1, true);     //visualization esdf map
    cloud_vis_pub = nh.advertise<sensor_msgs::PointCloud2>("/trans_cloud", 1, true); // visualization transform cloud
    current_goal_position_visual_pub = nh.advertise<visualization_msgs::Marker>("/current_goal_position_visual", 1);
    optimize_traj_visual_pub = nh.advertise<visualization_msgs::Marker>("/optimize_traj_visual", 1);
    a_star_pub = nh.advertise<nav_msgs::Path>("/a_star_path", 1);
    plan_time_pub = nh.advertise<visualization_msgs::Marker>("/PLAN_TIME", 10);
    optimize_traj_pub = nh.advertise<trajectory_msgs::JointTrajectory>("/optimize_traj", 1);
    pva_pub = nh.advertise<trajectory_msgs::JointTrajectoryPoint>("/pva_setpoint", 1);

    map_visual_timer_ = nh.createTimer(ros::Duration(MAP_VISUAL_TIME), &Formation_Agent::MapVisualCallback, this);
    exec_timer_ = nh.createTimer(ros::Duration(EXEC_TIME), &Formation_Agent::execFSMCallback, this);
    sendpva_timer = nh.createTimer(ros::Duration(0.03333), &Formation_Agent::sendPVA, this);

    init_head_yaw = 0;
    CLOUD_DELAY = 0;

    D435_T265_OFFSET_X = 0.0;     //D435在T265前方为正
    D435_T265_OFFSET_Z = 0.1;     //D435在T265上方为正

    D435_BASE_OFFSET_X = 0.0;     //D435在云台基座前方为正
    D435_BASE_OFFSET_Z = 0.0;    //D435在云台基座上方为正

    T265_OFFSET_X = 0.0;        //T265在无人机中心前方为正
    T265_OFFSET_Y = 0.0;        //T265在无人机中心左边为正
    T265_OFFSET_Z = 0.0;        //#T265在无人机中心上方为正

    CLIP_DISTANCE = 5;         //点云截断距离

    TAKEOFF_HEIGHT = 1.0; // 起飞高度

    current_p = Eigen::Vector3d::Zero();

    plan_end_p = Eigen::Vector3d::Zero();

    cloud_filtered.reset(new pcl::PointCloud<pcl::PointXYZ>());
    cloud_clip.reset(new pcl::PointCloud<pcl::PointXYZ>());

    global_goal << 1, 0, 0.8;
    local_goal << 0, 0, 0.8;

    stop_p << 0, 0, 0.1;

    plan_start_time = ros::Time::now();

    AgentInitFlag = true;
    exec_state_ = INIT;

    packagePath = ros::package::getPath("buaa");

    init_time = ros::Time::now().toSec();
    outFile.open(packagePath + "/cfg/plan_time.csv");  // TODO

    ROS_INFO("AGENT id:%d INIT---", AGENT_ID);

}

void Formation_Agent::gimbal_yaw_angleCallback(geometry_msgs::PoseStamped msg) {}

void Formation_Agent::cloudCallback(const sensor_msgs::PointCloud2ConstPtr &cloud) {
    if (NEED_GLOBAL_POS && !rec_global_goal_flag) return;
    if (!gvmInitial) return;
    ros::Time start = ros::Time::now();
    ROS_INFO_THROTTLE(4, "\033[1;%dm [CCB]: Agent %d, get point cloud\033[0m", COLOR_ID, AGENT_ID);

    double motor_yaw = 0;
    if (motor_yaw_queue.size() != 0) {
        for (int i = 0; i < motor_yaw_queue.size(); i++) {
            //Sychronize pose by queue
            double time_stamp_motor_yaw = motor_yaw_queue[i].header.stamp.toSec();
            if (time_stamp_motor_yaw >= cloud->header.stamp.toSec() - CLOUD_DELAY) {
                motor_yaw = motor_yaw_queue[i].pose.position.x - init_head_yaw;
                // ROS_INFO("motor_yaw:  %f",motor_yaw);
                if (fabs(cloud->header.stamp.toSec() - time_stamp_motor_yaw) > 0.010)
                    ROS_INFO_THROTTLE(3.0, "cloud mismatch time = %lf",
                                      cloud->header.stamp.toSec() - time_stamp_motor_yaw);
                break;
            }
        }
    }
    geometry_msgs::PoseStamped pose_sychronized;
    Eigen::Quaterniond quaternion_sychronized = current_q;  //w,x,y,z
    Eigen::Vector3d position_sychronized = current_p;

    while (!pose_queue.empty()) {
        pose_sychronized = pose_queue.front();
        if (pose_sychronized.header.stamp.toSec() >= cloud->header.stamp.toSec() - CLOUD_DELAY) {
            // ROS_INFO_THROTTLE(3.0, "cloud mismatch time = %lf", cloud->header.stamp.toSec() - pose_temp.header.stamp.toSec());
            // ROS_INFO("cloud->header.stamp.toSec(): %fpose_temp.header.stamp.toSec(): %f pose_timestamp_queue.back(): %f motor_yaw_time: %f motor_yaw: %f",
            //         cloud->header.stamp.toSec(),time_stamp_pose,pose_timestamp_queue.back(),motor_yaw_time,motor_yaw);
            quaternion_sychronized.x() = pose_sychronized.pose.orientation.x;
            quaternion_sychronized.y() = pose_sychronized.pose.orientation.y;
            quaternion_sychronized.z() = pose_sychronized.pose.orientation.z;
            quaternion_sychronized.w() = pose_sychronized.pose.orientation.w;

            position_sychronized(0) = pose_sychronized.pose.position.x;
            position_sychronized(1) = pose_sychronized.pose.position.y;
            position_sychronized(2) = pose_sychronized.pose.position.z;
            break;
        }
        pose_queue.pop();
    }

    Eigen::Matrix4d vehicle_to_vehicle_static = Eigen::Matrix4d::Identity();
    vehicle_to_vehicle_static.block(0, 0, 3, 3) = Eigen::Matrix3d(quaternion_sychronized);
    vehicle_to_vehicle_static(0, 3) = position_sychronized(0);  //front direction
    vehicle_to_vehicle_static(1, 3) = position_sychronized(1);      //left direction
    vehicle_to_vehicle_static(2, 3) = position_sychronized(2);  //up direction

    Eigen::Matrix4d d435base_to_vehicle = Eigen::Matrix4d::Identity();
    d435base_to_vehicle.block(0, 0, 3, 3) = Eigen::Matrix3d(
            Eigen::Quaterniond(cos(motor_yaw / 2.0), 0, 0, sin(motor_yaw / 2.0)));
    d435base_to_vehicle(0, 3) = 0.05;  //front direction
    d435base_to_vehicle(1, 3) = 0.0;  //left direction
    d435base_to_vehicle(2, 3) = 0.08; //up direction

    Eigen::Matrix4d d435_to_d435base = Eigen::Matrix4d::Identity();
    Eigen::Quaterniond d435_to_d435base_rotate(0.5, -0.5, 0.5, -0.5);
    d435_to_d435base.block(0, 0, 3, 3) = Eigen::Matrix3d(d435_to_d435base_rotate);
    d435_to_d435base(0, 3) = 0.0;
    d435_to_d435base(1, 3) = 0.0;
    d435_to_d435base(2, 3) = 0.0;

    Eigen::Matrix4d transform = vehicle_to_vehicle_static * d435base_to_vehicle * d435_to_d435base;
    // convert cloud to pcl form
    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_in(new pcl::PointCloud<pcl::PointXYZ>());
    pcl::fromROSMsg(*cloud, *cloud_in);

    // pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_sample(new pcl::PointCloud<pcl::PointXYZ>);
    // int number_sample = int(cloud_in->points.size() / 2.0);
    // pcl::RandomSample<pcl::PointXYZ> rs;
    // rs.setInputCloud(cloud_in);
    // rs.setSample(number_sample);
    // rs.filter(*cloud_sample);

    // pcl::PassThrough<pcl::PointXYZ> pass;
    // pass.setInputCloud(cloud_sample);
    // pass.setFilterFieldName("z");// 滤波字段设置为z轴方向
    // pass.setFilterLimits(-100, 0.15);
    // //设置保留范围内还是过滤掉范围内，true为过滤掉范围内的，fALse为过滤掉范围外的
    // pass.setFilterLimitsNegative(true);
    // pass.filter(*cloud_clip);

    pcl::VoxelGrid<pcl::PointXYZ> sor;
    sor.setInputCloud(cloud_in);
    double res = gvmcb.resolution_ / 2;
    sor.setLeafSize(res, res, res);
    sor.filter(*cloud_filtered);

    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_transform(new pcl::PointCloud<pcl::PointXYZ>());
    pcl::transformPointCloud(*cloud_filtered, *cloud_transform, transform);

    pcl::PassThrough<pcl::PointXYZ> pass;
    pass.setInputCloud(cloud_transform);
    pass.setFilterFieldName("z");// 滤波字段设置为z轴方向
    pass.setFilterLimits(-100, 0.05);
    //设置保留范围内还是过滤掉范围内，true为过滤掉范围内的，fALse为过滤掉范围外的
    pass.setFilterLimitsNegative(true);
    pass.filter(*cloud_clip);

    gvmcb.updateGridMapFromPointCLoud(cloud_clip);
    gvmcb.updateEDTMap();

    if (VISUAL_TRANS_CLOUD) {
        // pcl::transformPointCloud(*cloud_clip, *cloud_transform, transform);
        sensor_msgs::PointCloud2 cloud_visual;
        pcl::toROSMsg(*cloud_clip, cloud_visual);
        cloud_visual.header.stamp = ros::Time::now();
        cloud_visual.header.frame_id = "map";
        cloud_vis_pub.publish(cloud_visual);
    }
    ros::Time end = ros::Time::now();
    // std::cout<<"pc cb time: "<<(end-start).toSec()<<std::endl;
    // ROS_INFO("OUT POINTCLOUD CALLBACK");
}

void Formation_Agent::poseCallback(const geometry_msgs::PoseStampedPtr &msg) {
    current_p << msg->pose.position.x, msg->pose.position.y, msg->pose.position.z;
    current_q = Eigen::Quaterniond(msg->pose.orientation.w, msg->pose.orientation.x, msg->pose.orientation.y,
                                   msg->pose.orientation.z);

    geometry_msgs::PoseStamped store_msg = *msg;
    pose_queue.push(store_msg);
    if (pose_queue.size() > 10) {
        pose_queue.pop();
    }

    ros::Time before_gvmInitial = ros::Time::now();
    // ROS_INFO("BEFORE gvmInitial current_p: %f %f %f",current_p(0),current_p(1),current_p(2));
    if (!gvmInitial) {
        stop_p = current_p;
        gvmcb.setMapInitPose(current_p);
        gvmInitial = true;
        // ROS_INFO("initial gvmInitial");
    }
    gvmcb.updateOrigin(current_p);
    ros::Time after_gvmInitial = ros::Time::now();
    // ROS_INFO("AFTER  gvmInitial ,spend time: %f",(after_gvmInitial-before_gvmInitial).toSec());

    visualization_msgs::Marker meshROS;
    // Mesh model                                                  
    meshROS.header.frame_id = "map";
    meshROS.header.stamp = ros::Time::now();
    meshROS.ns = "mesh";
    meshROS.id = 2412;
    meshROS.type = visualization_msgs::Marker::MESH_RESOURCE;
    meshROS.action = visualization_msgs::Marker::ADD;
    meshROS.pose.position.x = current_p(0);
    meshROS.pose.position.y = current_p(1);
    meshROS.pose.position.z = current_p(2);

    meshROS.pose.orientation.x = current_q.x();
    meshROS.pose.orientation.y = current_q.y();
    meshROS.pose.orientation.z = current_q.z();;
    meshROS.pose.orientation.w = current_q.w();;
    double scale = 1.0;
    meshROS.scale.x = scale;
    meshROS.scale.y = scale;
    meshROS.scale.z = scale;
    meshROS.color.a = 1;
    meshROS.color.r = 0;
    meshROS.color.g = 0;
    meshROS.color.b = 0;
    std::string mesh_resource = "file://" + packagePath + "/rviz/hummingbird.mesh";
    meshROS.mesh_resource = mesh_resource;
    current_goal_position_visual_pub.publish(meshROS);
    // ROS_INFO("OUT POSE CALLBACK");
}

bool Formation_Agent::traj_optimization() {
    if (a_star_gridpath.size() == 0)return false;
    OptimizedTraj.clear();
    traj.points.clear();
    for (int i = 0; i < a_star_gridpath.size() - 1; i++) {

        Eigen::Vector3d path_point_start = gvmcb.getMapGlobalPosfromLocalIdx(a_star_gridpath[i]);
        Eigen::Vector3d path_point_end = gvmcb.getMapGlobalPosfromLocalIdx(a_star_gridpath[i + 1]);

        Eigen::Vector3d v_direction = (path_point_end - path_point_start).normalized();

        for (int time = 0;; time++) {
            Eigen::Vector3d traj_point = path_point_start + v_direction * time * 1.0 / rate * v;
            OptimizedTraj.push_back(traj_point);
            trajectory_msgs::JointTrajectoryPoint point;
            point.positions.push_back(traj_point(0));
            point.positions.push_back(traj_point(1));
            point.positions.push_back(traj_point(2));
            point.positions.push_back(0);

            point.velocities.push_back(v * v_direction(0));
            point.velocities.push_back(v * v_direction(1));
            point.velocities.push_back(v * v_direction(2));

            point.accelerations.push_back(0);
            point.accelerations.push_back(0);
            point.accelerations.push_back(0);

            traj.points.push_back(point);
            if (time * 1.0 / rate * v >= (path_point_end - path_point_start).norm())
                break;
        }
    }
    traj.header.stamp = ros::Time::now();
    optimize_traj_pub.publish(traj);
    // ROS_WARN("SEND TRAJ---------------");

    return true;
}


double getAngle(Eigen::Vector3d v1, Eigen::Vector3d v2) {
    double cosValNew = v1.dot(v2) / (v1.norm() * v2.norm()); //角度cos值
    double angleNew = acos(cosValNew);
    return angleNew;
}

void Formation_Agent::changeState(FSM_EXEC_STATE next_state) {
    exec_state_ = next_state;
}


void Formation_Agent::sendPVA(const ros::TimerEvent &e) {
    if (NEED_GLOBAL_POS && !rec_global_goal_flag) return;
    if (!gvmInitial)return;

    if (SET_POINT_CONTROL) {
        geometry_msgs::PoseStamped send_p;
        send_p.header.stamp = ros::Time::now();
        if (NEED_OFFBOARD_READY && !offboard_ready) {
            send_p.pose.position.x = current_p(0);
            send_p.pose.position.y = current_p(1);
            send_p.pose.position.z = current_p(2);
        } else if (!FLAG_TAKEOFF_DONE) {
            send_p.pose.position.x = takeoff_pose(0);
            send_p.pose.position.y = takeoff_pose(1);
            send_p.pose.position.z = takeoff_pose(2);
        } else if (a_star_flag == false) {
            send_p.pose.position.x = stop_p(0);
            send_p.pose.position.y = stop_p(1);
            send_p.pose.position.z = stop_p(2);
        } else {
            if (traj.points.size() == 0) {
                send_p.pose.position.x = current_p(0);
                send_p.pose.position.y = current_p(1);
                send_p.pose.position.z = current_p(2);
            } else {
                if (sendpva_count >= traj.points.size()) {
                    sendpva_count = traj.points.size() - 1;
                }
                send_p.pose.position.x = traj.points[sendpva_count].positions[0];
                send_p.pose.position.y = traj.points[sendpva_count].positions[1];
                send_p.pose.position.z = traj.points[sendpva_count].positions[2];
                sendpva_count++;
            }
        }
        setpoint_pub.publish(send_p);
    } else {
        trajectory_msgs::JointTrajectoryPoint point;
        if (exec_state_ == INIT || a_star_flag == false) {
            point.positions.push_back(stop_p(0));
            point.positions.push_back(stop_p(1));
            point.positions.push_back(stop_p(2));
            point.positions.push_back(0);
            point.velocities.push_back(0);
            point.velocities.push_back(0);
            point.velocities.push_back(0);
            point.accelerations.push_back(0);
            point.accelerations.push_back(0);
            point.accelerations.push_back(0);

        } else {
            if (traj.points.size() == 0) {
                return;
            }
            if (sendpva_count >= traj.points.size()) {
                sendpva_count = traj.points.size() - 1;
            }
            point = traj.points[sendpva_count];

            sendpva_count++;
        }
        point.time_from_start = ros::Duration(ros::Time::now().toSec());
        pva_pub.publish(point);

        // outFile << ros::Time::now().toSec()-init_time<<","<< point.positions[0] << ',' <<  point.positions[1] << ',' <<  point.positions[2]<<","
        //         << point.velocities[0] << ',' << point.velocities[1]  << ',' << point.velocities[2]<<","
        //         << point.accelerations[0] << ',' << point.accelerations[1]  << ',' <<point.accelerations[2] << ','<< a_star_flag<<","
        //         <<current_p(0)<<","<<current_p(1)<<","<<current_p(2)
        //         <<std::endl;
    }

}


void Formation_Agent::uavmodecallback(const mavros_msgs::State &msg) {
    if (msg.mode == "OFFBOARD")
        offboard_ready = true;
    else {
        offboard_ready = false;
        if(NEED_OFFBOARD_READY){
            changeState(INIT);
            traj.points.clear();
            sendpva_count = 0;
        }
    }
}

void Formation_Agent::GlobalGoalCallback(const geometry_msgs::PoseStampedConstPtr &msg) {
    if (NEED_GLOBAL_POS && rec_global_goal_flag) return;
    ROS_INFO("GET GOAL !!!!");
    ROS_INFO("GET GOAL !!!!");
    ROS_INFO("GET GOAL !!!!");
    ROS_INFO("GET GOAL !!!!");

    global_goal(0) = msg->pose.position.x;
    global_goal(1) = msg->pose.position.y;
    global_goal(2) = msg->pose.position.z + LAND_HEIGHT;
    rec_global_goal_flag = true;
    stop_p = current_p;
}

void Formation_Agent::save_traj_to_file() {
    outFile << "A new traj: " << traj.points.size() << std::endl;
    for (int i = sendpva_count_start; i < traj.points.size(); i++) {
        outFile << traj.points[i].positions[0] << ',' << traj.points[i].positions[1] << ','
                << traj.points[i].positions[2] << ","
                << traj.points[i].velocities[0] << ',' << traj.points[i].velocities[1] << ','
                << traj.points[i].velocities[2]
                << std::endl;
    }
    outFile.close();
    ROS_INFO_THROTTLE(4, " [SAVE]: Traj save to file...");
}


void Formation_Agent::execFSMCallback(const ros::TimerEvent &e) {
    // ROS_INFO("exec current_time: %f",ros::Time::now().toSec());
    if (!ENTER_EXEC_CALLBACK)return;
    if (NEED_GLOBAL_POS && !rec_global_goal_flag) return;
    if (!AgentInitFlag) return;
    if (!gvmInitial) return;

    switch (exec_state_) {
        case INIT: {
            ROS_INFO_THROTTLE(2, "\033[1;%dm [FSM]: INIT  MODE!!! \033[0m", 31);
            if (NEED_OFFBOARD_READY) {
                if (offboard_ready) {
                    changeState(TAKEOFF);
                }
            } else {
                changeState(TAKEOFF);
            }
            takeoff_pose = current_p;
            takeoff_pose(2) = TAKEOFF_HEIGHT;
            break;
        }

        case TAKEOFF: {
            ROS_INFO_THROTTLE(2, "\033[1;%dm [FSM]: TAKEOFF  MODE!!! \033[0m", 31);
            if (NEED_OFFBOARD_READY && !offboard_ready) {
                changeState(INIT);
            }
            double dist_to_goal = (takeoff_pose - current_p).norm();
            if (dist_to_goal < MIN_GOAL_DIS) {
                COUNT_TAKEOFF++;
                if (COUNT_TAKEOFF > TIME_TAKEOFF / EXEC_TIME) {
                    FLAG_TAKEOFF_DONE = true;
                    changeState(REPLAN_TRAJ);
                    break;
                }
            }
            break;
        }

        case EXEC_TRAJ: {
            ROS_INFO_THROTTLE(2, "\033[1;%dm [FSM]: EXEC_TRAJ MODE!!! \033[0m", 31);
            double dist_to_goal = (global_goal - current_p).norm();
            double dist_to_goal_xy = (global_goal - current_p).head<2>().norm(); // TODO

            if (dist_to_goal < MIN_GOAL_DIS || dist_to_goal_xy < MIN_GOAL_DIS * 2 / 3) {
                changeState(END_TRAJ);
                break;
            }
            bool tmp_flag = false;
            // 检查未来的轨迹是否危险
            for (int i = sendpva_count + 2; i < traj.points.size() - 1; i += 2) {
                if (i >= traj.points.size()) break;
                Eigen::Vector3d check_p(traj.points[i].positions[0], traj.points[i].positions[1],
                                        traj.points[i].positions[2]);
                if (gvmcb.getMapGlobalPosEdt(check_p) < SAFETY_DISTANCE / 1.5) {
                    a_star_flag = false;
                    stop_p = current_p;
                    changeState(REPLAN_TRAJ);
                    tmp_flag = true;
                    ROS_INFO("FRONT POINT DANGER!!!!! i: %d pos: %f %f %f distance: %f", i, check_p(0), check_p(1),
                             check_p(2), gvmcb.getMapGlobalPosEdt(check_p));
                    break;
                }
            }
            if (tmp_flag) break;

            Eigen::Vector3d local_goal_dist = current_p - local_goal;
            if (local_goal_dist.norm() < REPLAN_DIST) {
                // ROS_INFO(" get at local goal need to replan!!!!");
                changeState(REPLAN_TRAJ);
                break;
            }

            if ((ros::Time::now() - plan_start_time).toSec() > replan_time) {
                changeState(REPLAN_TRAJ);
                plan_start_time = ros::Time::now();
                // ROS_INFO(" TIME exceed replan , NEED TO replan!!!!");
            }
            break;
        }

        case REPLAN_TRAJ: {
            ROS_INFO_THROTTLE(2, "\033[1;%dm [FSM]: REPLAN_TRAJ MOde \033[0m", 31);
            ros::Time time_1 = ros::Time::now();
            double dist_to_goal = (global_goal - current_p).norm();
            double dist_to_goal_xy = (global_goal - current_p).head<2>().norm(); // TODO
//            std::cout << "Global Goal: "
//                      << global_goal(0) << " "
//                      << global_goal(1) << " "
//                      << global_goal(2) << std::endl; // TEST
            traj_start_time = ros::Time::now().toSec() - init_time;

            if (dist_to_goal < MIN_GOAL_DIS || dist_to_goal_xy < MIN_GOAL_DIS * 2 / 3) {
                changeState(END_TRAJ);
            } else {
                if (a_star_search(current_p, global_goal)) {
                    traj_optimization();
                    if (SAVE_TRAJ)
                        save_traj_to_file();
                    sendpva_count = sendpva_count_start;
                    first_fail = true;
                    plan_start_time = ros::Time::now();
                    stop_p = current_p;
                    double plan_time = (plan_start_time - time_1).toSec();
                    ROS_INFO_THROTTLE(4, "\033[1;%dm [PLAN]: Time of path planning is %fs \033[0m", 34, plan_time);
                    outFile << plan_time << std::endl;

                    visualization_msgs::Marker marker;
                    marker.header.frame_id = "map";
                    marker.header.stamp = ros::Time::now();
                    marker.ns = "lines_and_points";
                    marker.action = visualization_msgs::Marker::ADD;
                    marker.pose.orientation.w = 1.0;
                    marker.id = 543213;
                    marker.type = visualization_msgs::Marker::TEXT_VIEW_FACING;
                    marker.scale.z = 0.2;
                    marker.color.b = 0;
                    marker.color.g = 0;
                    marker.color.r = 255;
                    marker.color.a = 1;
                    marker.pose.position.x = current_p(0);
                    marker.pose.position.y = current_p(1) - 0.05;
                    marker.pose.position.z = current_p(2);
                    ostringstream str;
                    str << "plan path use time: " << plan_time << "s";
                    marker.text = str.str();
                    plan_time_pub.publish(marker);
                    // Switch to EXEC
                    changeState(EXEC_TRAJ);
                } else {
                    if (first_fail) {
                        stop_p = current_p;
                    }
                    first_fail = false;
                }
            }
            break;
        }

        case END_TRAJ: {
            ROS_INFO_THROTTLE(2, "\033[1;%dm [FSM]: END_TRAJ  MODE \033[0m", 31);
            double dist_to_goal = (global_goal - current_p).norm();
            double dist_to_goal_xy = (global_goal - current_p).head<2>().norm(); // TODO

            if (dist_to_goal < MIN_GOAL_DIS || dist_to_goal_xy < MIN_GOAL_DIS * 2 / 3) {
                traj.points.clear();

                trajectory_msgs::JointTrajectoryPoint point;
                point.positions.push_back(global_goal(0));
                point.positions.push_back(global_goal(1));
                point.positions.push_back(global_goal(2));
                point.positions.push_back(0);

                point.velocities.push_back(0);
                point.velocities.push_back(0);
                point.velocities.push_back(0);

                point.accelerations.push_back(0);
                point.accelerations.push_back(0);
                point.accelerations.push_back(0);

                traj.points.push_back(point);
                sendpva_count = 0;
                break;
            } else {
                changeState(REPLAN_TRAJ);
            }
            break;
        }
        case EMERGENCY_STOP: {
            break;
        }
    }
}
